/** ###################################################################
**     Filename  : inc.c
**     Project   : inc
**     Processor : MC9S12XDT512BMFU
**     Version   : Driver 01.14
**     Compiler  : CodeWarrior HCS12X C Compiler
**     Date/Time : 24.10.2011, 11:30
**     Abstract  :
**         Main module.
**         This module contains user's application code.
**     Settings  :
**     Contents  :
**         No public methods
**
** ###################################################################*/
/* MODULE inc */

/* Including needed modules to compile this module/procedure */
#include "Cpu.h"
#include "Events.h"
#include "TI1.h"
#include "AS1.h"
#include "GPIO1.h"
/* Include shared modules, which are used for whole project */
#include "PE_Types.h"
#include "PE_Error.h"
#include "PE_Const.h"
#include "IO_Map.h"

/* User includes (#include below this line is not maintained by Processor Expert) */
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <math.h>
#include <ctype.h>

#define TLT_ID 0x02

typedef struct 
{
 word id;
 byte len;
 byte data[8];
} can_message_t;

byte can_recv( can_message_t *pmess);
byte can_send( can_message_t *pmess);
void can_init( void);

#define LED0 PORTA_PA7
#define LED1 PORTA_PA6

#define CANE   0x80
#define INITAK 0x01
#define INITRQ 0x01
#define SLPRQ  0x02
#define SLPAK  0x02

byte timer_int = 0;
byte inc_prot = 0;
char inc_buf[10] = "";
byte inc_buf_index = 0;

byte req_cnt = 0;
byte meas_req = 1;
byte measured_flag = 0;

// converts a hexadecimal string to integer
byte xtoi( const char* xs, unsigned int* result)
{
  size_t szlen = strlen( xs);
  int i, xv, fact;

  if( szlen > 0)
  {
    // converting more than 32bit hexadecimal value
    if( szlen > 8)
    {
      return 0;
    }

    // begin conversion here
    *result = 0;
    fact = 1;

    // run until no more character to convert
    for( i = szlen - 1; i >= 0 ; i--)
    {
      if( isxdigit( *( xs + i)))
      {
        if( *( xs + i) >= 97)
        {
          xv = ( *( xs+i) - 97) + 10;
        }
        else if ( *( xs + i) >= 65)
        {
          xv = (*( xs + i) - 65) + 10;
        }
        else
        {
          xv = *( xs + i) - 48;
        }
        *result += (xv * fact);
        fact *= 16;
      }
      else
      {
        return 0;
      }
    }
 }
 
 return 1;
}

void main( void)
{
  byte ret;
  unsigned int num;
  float f;
  byte c;
  can_message_t msg;

  float pitch_buf;
  float pitch;
  float roll;
  
  PE_low_level_init();
  
  
       Cpu_Delay100US(40000);
  
  

  LED0 = 1;
  LED1 = 1;
  
  can_init();
  
  measured_flag = 1;
  inc_buf_index = 0;
  inc_prot = 0;
  for( ;;)
  {
    // data receive
    ret = AS1_RecvChar( &c);
    if( ret == ERR_OK)
    {
      if( inc_buf_index < 4)
      {
        inc_buf[inc_buf_index] = c; // save byte into buffer
        inc_buf_index++; // increse buffer index
      }

      // 4 bytes received
      if( inc_buf_index >= 4)
      {
        // 4. byte = cr
        if( inc_buf[3] == 10)
        {
          inc_buf[3] = 0;
          ( void) xtoi( &inc_buf[0], &num); // convert value from string (in hex format) to short numeric data type
          f = ( float) num;
          LED0 = ~LED0;
          
          // save value acording to actual value type
          switch( inc_prot)
          {
            case 0: // pitch
              pitch_buf = ( f - 2048.0f) * 0.0316f;
              ( void) AS1_SendChar( 'C'); while( AS1_GetTxComplete() == 0) {};
              ( void) AS1_SendChar( '\r'); while( AS1_GetTxComplete() == 0) {};
              ( void) AS1_SendChar( '\n'); while( AS1_GetTxComplete() == 0) {};
              inc_buf_index = 0;
              inc_prot = 1;
            break;

            case 1: // roll
              pitch = pitch_buf;
              roll = ( f - 2048.0f) * 0.0316f;
              inc_prot = 0;
              measured_flag = 1;
            break;
        
            default:
              inc_prot = 0;
            break;
          }
        }
        inc_buf_index = 0;
      }
    }
    
    // measure request
    if( meas_req == 1)
    {
      meas_req = 0;
      // pitch
      ( void) AS1_SendChar( 'B'); while( AS1_GetTxComplete() == 0) {};
      ( void) AS1_SendChar( '\r'); while( AS1_GetTxComplete() == 0) {};
      ( void) AS1_SendChar( '\n'); while( AS1_GetTxComplete() == 0) {};
      inc_prot = 0;
      inc_buf_index = 0;
    }
    
    // can request
    ret = can_recv( &msg);
    if( ret == 1)
    {  
      CAN0RFLG = 0x01;     
      if( msg.id == 1)
      {
        req_cnt++;
        if( req_cnt >= 20)
        {
          meas_req = 1;
          req_cnt = 0;
        }
        if( measured_flag == 1)
        if( msg.data[0] == 1)
        {
          meas_req = 1;
          measured_flag = 0;
          req_cnt = 0;
          
          msg.id = 0x138;
          msg.len = 8;
          msg.data[0] = TLT_ID;
          msg.data[1] = 2;
          msg.data[2] = 0;
          msg.data[3] = 0;
          msg.data[4] = ( ( unsigned char*) &pitch)[0];
          msg.data[5] = ( ( unsigned char*) &pitch)[1];
          msg.data[6] = ( ( unsigned char*) &pitch)[2];
          msg.data[7] = ( ( unsigned char*) &pitch)[3];
          while( !can_send( &msg));

          msg.id = 0x137;
          msg.len = 8;
          msg.data[0] = TLT_ID;
          msg.data[1] = 2;
          msg.data[2] = 0;
          msg.data[3] = 0;
          msg.data[4] = ( ( unsigned char*) &roll)[0];
          msg.data[5] = ( ( unsigned char*) &roll)[1];
          msg.data[6] = ( ( unsigned char*) &roll)[2];
          msg.data[7] = ( ( unsigned char*) &roll)[3];
          while( !can_send( &msg));
          
          LED1 = ~LED1;
        }
        else
        {
          msg.id = 0x137;
          msg.len = 1;
          msg.data[0] = TLT_ID;
          while( !can_send( &msg));

          msg.id = 0x138;
          msg.len = 1;
          msg.data[0] = TLT_ID;
          while( !can_send( &msg));
        }
      }
    }    
  }

  /*** Processor Expert end of main routine. DON'T MODIFY THIS CODE!!! ***/
  for(;;){}
  /*** Processor Expert end of main routine. DON'T WRITE CODE BELOW!!! ***/
} /*** End of main routine. DO NOT MODIFY THIS TEXT!!! ***/

byte can_recv( can_message_t *pmess)
{
  byte i, tmp;
  
  if( CAN0RFLG & 0x01) 
  {
    pmess->id = CAN0RXIDR0 << 8;
    pmess->id += CAN0RXIDR1;
    if( pmess->id & 0x0010) 
    { 
      pmess->len = 0x80;
    }
    else
    {
      pmess->len = 0;
    }
    
    pmess->id = ( pmess->id >> 5) & 0x07ff;
                                                                                                     
    if( ( CAN0RXDLR & 0x0f) > 8) 
    {
      pmess->len |= 0x08;
      tmp = 8;
    }
    else	
    {
      tmp = CAN0RXDLR & 0x0f;
      pmess->len |= tmp;  
    }
    
    for( i=0; i < tmp; i++)
    {
      pmess->data[i] = (&CAN0RXDSR0)[i];
    }
    return 1;
  }
  return 0;
}

byte can_send( can_message_t *pmess)
{
  char i;
  unsigned short volatile *pireg;
 
  if( CAN0TFLG & 0x01)
  {
    pireg = ( unsigned short volatile *)( &CAN0TXIDR0);
    if( pmess->len & 0x80)
    { 
      *pireg = ( ( pmess->id << 5) & 0xffe0) | 0x0010;	// write id, rtr frame
    }
    else
    {
      *pireg = ( ( pmess->id << 5) & 0xffe0); // write id, data frame
    }
    pireg++;
    *pireg = 0;
    
    pmess->len &= 0x0f;
    for( i = 0; i < pmess->len; i++)
    {
      ( &CAN0TXDSR0)[i] = pmess->data[i];	  // write data
    }
    CAN0TXDLR = pmess->len; 	        // write dlc
    CAN0TFLG = 1;	 					  // start transmission
    return 1;
  }

  return 0;
}

void can_init(void)
{
  CAN0CTL0 |= INITRQ; // set initrq, this will also set initak
  while( !( CAN0CTL1 & INITAK)); // wait for init mode to occur

  CAN0CTL1 = CANE; // set cane just in case this is the first time after reset

  CAN0BTR0=0x01; // for 500kbps 0x03, for 100kbps 0x13
  CAN0BTR1=0x32; // for a can Baud of 500kbps at 16Mhz crystal

  // can id filter bits
  CAN0IDAR0=0x00;
  CAN0IDAR1=0x00;
  CAN0IDAR2=0x00;
  CAN0IDAR3=0x00;
  CAN0IDAR4=0x00;
  CAN0IDAR5=0x00;
  CAN0IDAR6=0x00;
  CAN0IDAR7=0x00;

  // can id filter mask
  CAN0IDMR0=0xff;
  CAN0IDMR1=0xff;
  CAN0IDMR2=0xff;
  CAN0IDMR3=0xff;
  CAN0IDMR4=0xff;
  CAN0IDMR5=0xff;
  CAN0IDMR6=0xff;
  CAN0IDMR7=0xff;

  CAN0IDAC = 0x10; // set filters to 2 16 bit acceptance filters

  CAN0CTL0 &= ~INITRQ; // clear initrq
  while (CAN0CTL1 & INITAK);

  CAN0CTL1 |= CANE; // set cane just in case this is the first time after reset
  CAN0RIER_RXFIE = 0; // enable receive interrupt

  CAN0TBSEL = 1;
}

/* END inc */
/*
** ###################################################################
**
**     This file was created by Processor Expert 3.02 [04.44]
**     for the Freescale HCS12X series of microcontrollers.
**
** ###################################################################
*/
